#include "../include/xtark_driver/xtark_driver.h"

XtarkDriver::XtarkDriver():msg_seq_(0),start_flag_(true),state_(waitingForHead1),servo_bias_(0),first_init_(false){}
XtarkDriver::~XtarkDriver()
{
}
/*动态参数配置服务回调函数*/
/*主循环函数*/
void XtarkDriver::loop()
{
    uint8_t stop_buf[13];
    ros::NodeHandle nh_;
    ros::NodeHandle nh_p_("~");

    /*从配置文件中获取机器人参数*/
    nh_p_.param<std::string>("port_name",port_name_,std::string("/dev/ttyUSB0"));
    nh_p_.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    nh_p_.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    nh_p_.param<std::string>("imu_frame",imu_frame_,std::string("base_imu_link"));

    nh_p_.param<int>("baud_rate",baud_rate_,115200);
    nh_p_.param<int>("control_rate",control_rate_,50);
    
    nh_p_.param<double>("linear_correction_factor",linear_correction_factor_,1.0);
    nh_p_.param<int>("servo_bias",servo_bias_,0);

    nh_p_.param<bool>("publish_odom_transform",publish_odom_transform_,true);
    nh_p_.param<int>("Kp",kp_,300);
    nh_p_.param<int>("Ki",ki_,0);
    nh_p_.param<int>("Kd",kd_,200);
    

    odom_list_.resize(6,0.0);
    imu_list_.resize(9,0.0);
    wheelspeedSet_list_.resize(2,0);
    wheelspeedGet_list_.resize(2,0);
    /*初始化机器人硬件串口以及publisher发布器和定时器与动态配置参数服务器等的回调函数，然后进入ros::spin()循环*/
    if(ros::ok())
    {
        odom_pub_    = nh_.advertise<nav_msgs::Odometry>("odom",10);
        battery_pub_ = nh_.advertise<std_msgs::Float32>("voltage",1);
        imu_pub_     = nh_.advertise<sensor_msgs::Imu>("imu",10);
        lvel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/lvel",10);
        rvel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/rvel",10);
        lset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/lset",10);
        rset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/rset",10);
        cmd_sub_     = nh_.subscribe<geometry_msgs::Twist>("cmd_vel",10,&XtarkDriver::cmd_vel_callback,this);
        
        //SetParams(linear_correction_factor_,servo_bias_);
        //ros::Duration(0.02).sleep();
        //SetPID(kp_,ki_,kd_);

        //ros::Timer send_speed_timer = nh_.createTimer(ros::Duration(1.0/control_rate_),&XtarkDriver::send_speed_callback,this);

        //ros::timer

        boost::thread recv_thread(boost::bind(&XtarkDriver::recv_msg,this));
        //boost::thread recv_thread(boost::bind(&XtarkDriver::handle_base_data,this));

	    //dynamic_reconfigure::Server<xtark_driver::PID_reconfigConfig> reconfig_server;   ///////
	    //dynamic_reconfigure::Server<xtark_driver::PID_reconfigConfig>::CallbackType f;   ///////

	    //f = boost::bind(&XtarkDriver::dynamic_reconfig_callback,this,_1,_2);
	    //reconfig_server.setCallback(f);
        ROS_INFO("Robot Running!");
        ros::spin();
    	//SetVelocity(0,0,0);
        return ;
    }
}
/*初始化硬件端口*/
/*将收到的/cmd_vel上的速度话题消息通过串口发送给机器人*/
/*cmd_vel Subscriber的回调函数*/
/*串口通信校验位计算辅助函数*/
/*与OpenCRP串口通信接收数据线程*/
/*数据校验分发*/
/*PID参数发送函数*/
/*底盘参数发送函数*/
/*底盘速度发送函数*/
/*收到串口数据包解析函数*/
void XtarkDriver::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{   
    int n =1;
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(...)
    {
        cmd_vel_mutex_.unlock();
    }
}
void XtarkDriver::recv_msg()
{   
    std::cout<<"123"<<std::endl;
    int m = 1; 
    uint8_t payload_size, check_num, buffer_data[255],payload_type;
    recv_flag_ = true;
    while(recv_flag_)
    {
        for(int i=0; i<256;i++)
            {
                buffer_data[i]=0;
            }
        distribute_data(payload_type, buffer_data);
    }
}

void XtarkDriver::distribute_data(uint8_t msg_type, uint8_t* buffer_data)
{
     if(true)
        handle_base_data(buffer_data);
}
void XtarkDriver::handle_base_data(const uint8_t* buffer_data)
//void XtarkDriver::handle_base_data()
{
        now_ = ros::Time::now();
        // uint8_t payload_size, check_num, buffer_data[255],payload_type;
        // for(int i=0; i<256;i++)
        //     {
        //         buffer_data[i]=0;
        //     }        
        //gyro
        //std::cout<<"pub......pub"<<std::endl;
        imu_list_[0]=((double)((int16_t)(buffer_data[4]*256+buffer_data[5]))/32768*2000/180*3.1415);
        imu_list_[1]=((double)((int16_t)(buffer_data[6]*256+buffer_data[7]))/32768*2000/180*3.1415);
        imu_list_[2]=((double)((int16_t)(buffer_data[8]*256+buffer_data[9]))/32768*2000/180*3.1415);
        //Acc 
        imu_list_[3]=((double)((int16_t)(buffer_data[10]*256+buffer_data[11]))/32768*2*9.8);
        imu_list_[4]=((double)((int16_t)(buffer_data[12]*256+buffer_data[13]))/32768*2*9.8);
        imu_list_[5]=((double)((int16_t)(buffer_data[14]*256+buffer_data[15]))/32768*2*9.8);
        //Angle 
        imu_list_[6]=((double)((int16_t)(buffer_data[16]*256+buffer_data[17]))/100);
        imu_list_[7]=((double)((int16_t)(buffer_data[18]*256+buffer_data[19]))/100);
        imu_list_[8]=((double)((int16_t)(buffer_data[20]*256+buffer_data[21]))/100);
        imu_pub_data_.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,(imu_list_[8]/180*3.1415926));
        imu_pub_data_.header.stamp = ros::Time::now();
        imu_pub_data_.header.frame_id = imu_frame_;
        imu_pub_data_.angular_velocity.x = imu_list_[0];
        imu_pub_data_.angular_velocity.y = imu_list_[1];
        imu_pub_data_.angular_velocity.z = imu_list_[2];
        imu_pub_data_.linear_acceleration.x = imu_list_[3];
        imu_pub_data_.linear_acceleration.y = imu_list_[4];
        imu_pub_data_.linear_acceleration.z = imu_list_[5];
        imu_pub_data_.orientation_covariance = {1e6, 0, 0,
                                            0, 1e6, 0,
                                            0, 0, 0.02};
        imu_pub_data_.angular_velocity_covariance = {1e6, 0, 0,
                                                 0, 1e6, 0,
                                                 0, 0, 1e6};
        imu_pub_data_.linear_acceleration_covariance = {1e-2, 0, 0,
                                                     0, 0, 0,
                                                     0, 0, 0};
        //imu_pub_.publish(imu_pub_data_);

        odom_list_[0]=((double)((int16_t)(buffer_data[22]*256+buffer_data[23]))/1000);
        odom_list_[1]=((double)((int16_t)(buffer_data[24]*256+buffer_data[25]))/1000);
        odom_list_[2]=((double)((int16_t)(buffer_data[26]*256+buffer_data[27]))/1000);
        //dx dy dyaw base_frame
        odom_list_[3]=((double)((int16_t)(buffer_data[28]*256+buffer_data[29]))/1000);
        odom_list_[4]=((double)((int16_t)(buffer_data[30]*256+buffer_data[31]))/1000);
        odom_list_[5]=((double)((int16_t)(buffer_data[32]*256+buffer_data[33]))/1000);
        transformStamped_.header.stamp    = now_;
        transformStamped_.header.frame_id = odom_frame_;
        transformStamped_.child_frame_id  = base_frame_;
        transformStamped_.transform.translation.x = odom_list_[0];
        transformStamped_.transform.translation.y = odom_list_[1];
        transformStamped_.transform.translation.z = 0.0;
        tf2::Quaternion q;
        q.setRPY(0,0,odom_list_[2]);
        transformStamped_.transform.rotation.x = q.x();
        transformStamped_.transform.rotation.y = q.y();
        transformStamped_.transform.rotation.z = q.z();
        transformStamped_.transform.rotation.w = q.w();
        if(publish_odom_transform_)
            br_.sendTransform(transformStamped_);

        odom_.header.frame_id = odom_frame_;
        odom_.child_frame_id  = base_frame_;
        odom_.header.stamp    = now_;
        odom_.pose.pose.position.x = odom_list_[0];
        odom_.pose.pose.position.y = odom_list_[1];
        odom_.pose.pose.position.z = 0;
        odom_.pose.pose.orientation.x = q.getX();
        odom_.pose.pose.orientation.y = q.getY();
        odom_.pose.pose.orientation.z = q.getZ();
        odom_.pose.pose.orientation.w = q.getW();
        /*
        odom_.twist.twist.linear.x = odom_list_[3]/((now_-last_time_).toSec());
        odom_.twist.twist.linear.y = odom_list_[4]/((now_-last_time_).toSec());
        odom_.twist.twist.angular.z = odom_list_[5]/((now_-last_time_).toSec());
        
        odom_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 0.10 };
        */
        odom_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e3 };
        odom_pub_.publish(odom_);
        last_time_ = now_;

        wheelspeedGet_list_[0]=((int16_t)(buffer_data[34]*256+buffer_data[35]));
        wheelspeedGet_list_[1]=((int16_t)(buffer_data[36]*256+buffer_data[37]));
        wheelspeedSet_list_[0]=((int16_t)(buffer_data[38]*256+buffer_data[39]));
        wheelspeedSet_list_[1]=((int16_t)(buffer_data[40]*256+buffer_data[41]));
        lvel_pub_data_.data = wheelspeedGet_list_[0];
        rvel_pub_data_.data = wheelspeedGet_list_[1];
        lset_pub_data_.data = wheelspeedSet_list_[0];
        rset_pub_data_.data = wheelspeedSet_list_[1];
        lvel_pub_.publish(lvel_pub_data_);
        rvel_pub_.publish(rvel_pub_data_);
        lset_pub_.publish(lset_pub_data_);
        rset_pub_.publish(rset_pub_data_);
        battery_pub_data_.data = (double)(((buffer_data[42]<<8)+buffer_data[43]))/100;
        battery_pub_.publish(battery_pub_data_);
}
int main(int argc,char** argv)//
{
    ros::init(argc,argv,"Xtark_driver_node");
    XtarkDriver driver;
    driver.loop();
    return 0;
}
